#ifndef __FAST_DDS_H__
#define __FAST_DDS_H__

#include "BaseType.h"
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/Domain.h>
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/attributes/SubscriberAttributes.h>

#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/publisher/PublisherListener.h>

#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/subscriber/SubscriberListener.h>
#include <fastrtps/subscriber/SampleInfo.h>

#include <functional>

namespace libembx{
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;

template <typename DDSMsg,typename DDSMsgType>
class FastTalker: public PublisherListener{
DECL_CLASSNAME(FastTalker)
private:
	Participant *m_participant{NULL};
	Publisher *m_publisher{NULL};
	int m_nMatched{0};
	DDSMsgType m_msgType;
public:
	FastTalker(){};
	~FastTalker()
	{
		Domain::removeParticipant(m_participant);
	}
	/* 如果是与ROS2通信,topic需要加上对应的前缀"rt/",否则发送的消息ROS2无法收到 */
	bool init(std::string name,std::string topic, int domain=-1)
	{
		ParticipantAttributes PParam;
    	PParam.rtps.setName(CSTR(name));
		if (domain>=0)
    	{
    		PParam.rtps.builtin.domainId=domain;
		}
    	m_participant = Domain::createParticipant(PParam);
    	if(m_participant==NULL)
    	{
    		TRACE_ERR_CLASS("createParticipant error!\n");
        	return false;
    	}
	    Domain::registerType(m_participant, static_cast<TopicDataType*>(&m_msgType));
	    PublisherAttributes Wparam;
	    Wparam.topic.topicKind = NO_KEY;
	    Wparam.topic.topicDataType = m_msgType.getName();
	    Wparam.topic.topicName = CSTR(topic);
	    m_publisher = Domain::createPublisher(m_participant,Wparam,static_cast<PublisherListener*>(this));
	    if(m_publisher == NULL)
	    {
	    	TRACE_ERR_CLASS("createPublisher error!\n");
	        return false;
	    }
	    return true;
	}
	int getListeners()
	{
		return m_nMatched;
	}
	void sendMsg(void* msg)
	{
		if (m_publisher!=NULL)
		{
			m_publisher->write(msg);
		}
	}
private:
	void onPublicationMatched(Publisher* pub,MatchingInfo& info)
	{
		if (info.status == MATCHED_MATCHING)
		{
			m_nMatched++;
			//TRACE_INFO_CLASS("Publisher matched\n");
		}
		else
		{
			m_nMatched--;
			//TRACE_INFO_CLASS("Publisher unmatched\n");
		}
	}
};

using DDSCallBackOnNewMsg = std::function<void(void*)>;
template <typename DDSMsg,typename DDSMsgType>
class FastListener:public SubscriberListener{
DECL_CLASSNAME(FastListener)
private:
	Participant *m_participant{NULL};
	Subscriber *m_subscriber{NULL};
	int m_nMatched{0};
	DDSCallBackOnNewMsg m_onNewMsg;
	DDSMsgType m_msgType;
public:
	FastListener(){};
	~FastListener()
	{
		Domain::removeParticipant(m_participant);
	}
	/* 如果是与ROS2通信,topic需要加上对应的前缀"rt/",否则无法接收到ROS2的消息 */
	bool init(std::string name, std::string topic,DDSCallBackOnNewMsg onNewMsg,int domain=-1)
	{
		ParticipantAttributes PParam;
	    PParam.rtps.setName(CSTR(name));
		if (domain>=0)
	    {
	    	PParam.rtps.builtin.domainId=domain;
		}
	    m_participant = Domain::createParticipant(PParam);
	    if(m_participant == NULL)
	    {
	    	TRACE_ERR_CLASS("createParticipant error!\n");
	        return false;
	    }
	    Domain::registerType(m_participant, static_cast<TopicDataType*>(&m_msgType));
	    SubscriberAttributes Rparam;
	    Rparam.topic.topicKind = NO_KEY;
	    Rparam.topic.topicDataType = m_msgType.getName();
	    Rparam.topic.topicName = CSTR(topic);
	    m_subscriber = Domain::createSubscriber(m_participant,Rparam, static_cast<SubscriberListener*>(this));
	    if(m_subscriber == NULL)
	    {
	    	TRACE_ERR_CLASS("createSubscriber error!\n");
	        return false;
	    }
		m_onNewMsg = onNewMsg;
	    return true;
	}
private:
	void onSubscriptionMatched(Subscriber* sub,MatchingInfo& info)
	{
	    if (info.status == MATCHED_MATCHING)
	    {
	        m_nMatched++;
	        //TRACE_INFO_CLASS("Subscriber matched\n");
	    }
	    else
	    {
	        m_nMatched--;
	        //TRACE_INFO_CLASS("Subscriber unmatched\n");
	    }
	}
	void onNewDataMessage(Subscriber* sub)
	{
		SampleInfo_t m_info;
		DDSMsg msg;
	 	if(sub->takeNextData(&msg, &m_info))
	    {
	        if(m_info.sampleKind == ALIVE)
	        {
	        	if (m_onNewMsg!=NULL)
				{
					m_onNewMsg(&msg);
	        	}
	        }
	    }
	}
};
}
#endif
